//
// Created by hrg on 2019/12/3.
//
#include <ecdata.h>

EcData::EcData(int groupnum): group_count_(groupnum){
	servo_count_ = 0;
	data_blks_ = NULL;
	free_ptr_ = NULL;
    curr_ptr_ = NULL;
    next_ptr_ = NULL;
}

EcData::~EcData() {
    if (data_blks_ != nullptr) {
        delete [] data_blks_;
        data_blks_ = nullptr;
    }
}

int* EcData::DataLoadFromCurrBlock(void) {
    return curr_ptr_;
}

void EcData::Init(int slvnum) {
	servo_count_ = slvnum;
    data_blks_ = new int[DATA_BLOCK_NUM_DEFAULT * group_count_ * servo_count_];
    free_ptr_ = data_blks_;
}

void EcData::Clear() {
    next_ptr_ = nullptr;
    curr_ptr_ = nullptr;
    free_ptr_ = data_blks_;
}

/* Transfer data from next_ptr_ to cur_ptr. */
void EcData::DataBlockTransfToCurPtr(void) {
    if(next_ptr_ == nullptr){
        curr_ptr_ = nullptr;
    } else {
        curr_ptr_ = next_ptr_;
        next_ptr_ = nullptr;
    }
}

bool EcData::DataNextBlockEmpty(){
    if(next_ptr_ == NULL){
        return true;
    }
    return false;
}

/* Load data into free data block, and transfer it from free_ptr_ to next_ptr_. */
void EcData::DataStoreIntoNextBlock(int *data) {
    int len = group_count_ * servo_count_ * sizeof(int);
    memcpy((void*)free_ptr_, (void *)data, len);

    /* Transfer data from free_ptr_ to next_ptr. */
    next_ptr_ = free_ptr_;

    if(free_ptr_ == data_blks_)
        free_ptr_ += group_count_ * servo_count_;
    else
        free_ptr_ = data_blks_;
    return;
}
